#20250108 at1730  7.4.4调用接口进行路点导航
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator,TaskResult
import rclpy
from rclpy.duration import Duration

def main():
    rclpy.init()
    navigator = BasicNavigator() #节点
    navigator.waitUntilNav2Active() #等待导航可用
    
    #路点数组
    goal_poses =[]

    #第一个点
    goal_pose = PoseStamped() #消息接口：：ros2 action send_goal  /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose:{header: {frame_id: map} ,pose: { position: {x: 2.0, y: 2.0}}}}" --feedback 
    goal_pose.header.frame_id = 'map'
    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose.pose.position.x = 2.0
    goal_pose.pose.position.y = 1.0
    goal_pose.pose.orientation.w = 1.0

    #加入数组
    goal_poses.append(goal_pose)
    
    #第2个点
    goal_pose2 = PoseStamped() #消息接口：：ros2 action send_goal  /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose:{header: {frame_id: map} ,pose: { position: {x: 2.0, y: 2.0}}}}" --feedback 
    goal_pose2.header.frame_id = 'map'
    goal_pose2.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose2.pose.position.x = 1.0
    goal_pose2.pose.position.y = 1.0
    goal_pose2.pose.orientation.w = 1.0

    #加入数组
    goal_poses.append(goal_pose2)


     #第3个点
    goal_pose3 = PoseStamped() #消息接口：：ros2 action send_goal  /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose:{header: {frame_id: map} ,pose: { position: {x: 2.0, y: 2.0}}}}" --feedback 
    goal_pose3.header.frame_id = 'map'
    goal_pose3.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose3.pose.position.x = 0.0
    goal_pose3.pose.position.y = 0.0
    goal_pose3.pose.orientation.w = 1.0

    #加入数组
    goal_poses.append(goal_pose3)
    #
    #发送目标位置，接受反馈：
    navigator.followWaypoints(goal_poses)
    
    while not navigator.isTaskComplete():
        feedback = navigator.getFeedback()
        navigator.get_logger().info(f'当前路点编号：{feedback.current_waypoint}')
       

     
    
    
    # navigator.setInitialPose(goal_pose)#方法
    # 最终结果判断
    result = navigator.getResult()
    if result == TaskResult.SUCCEEDED: 
        navigator.get_logger().info('导航结果：成功')
    elif result == TaskResult.CANCELED:
        navigator.get_logger().warn('导航结果：被取消')
    elif result == TaskResult.FAILED:
        navigator.get_logger().error('导航结果：失败')
    else:
        navigator.get_logger().error('导航结果：返回状态无效')



    #rclpy.spin(navigator)
    #rclpy.shutdown()

if __name__ == '__main__':
    main()
